/*
 */

#include <Arduino.h>

#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"
#include "MotorDriver.h"

#define CHANNEL (81)	      // something the local WiFi doesn't spam
#define PACKETSIZE (26)
const uint8_t* address = "MOMO";
uint8_t payload[PACKETSIZE];
RF24 radio (48, 49);

#define TRACKLEFT (1)
#define TRACKRIGHT (2)
MotorDriver motor;

#define CAMERA Serial1
uint8_t buffer[PACKETSIZE+10]; // takes the longest init output string

#define DISTSEN (A15)

// ----------------------------------------------------------------------

void proximity (void)
{
  float distance = 0;

  for (int i = 10; i; i--)
    {
      distance += 10650.08 * powf (analogRead(DISTSEN), -0.935) - 10;
      delay (20);
    }
  distance = distance / 10.0;

  memset (payload, 32, PACKETSIZE);
  sprintf (payload, "Proximity=%d", (int)distance);
  for (int i = 0; i < PACKETSIZE; i++)
    if (payload[i] == 0) payload[i] = 32;

  radio.stopListening();
  radio.write (payload, PACKETSIZE);
  radio.startListening();
}

// ----------------------------------------------------------------------

uint8_t READ_FBUF[] =
  { 0x56, 0x00, 0x32, 0x0C,
    0x00,			// frame type (00=current frame)
    0x0A,			// control mode 1110 data xfer by MCU
    0x00, 0x00, 0x00, 0x00,	// 6-9: starting address, always 0
    0x00, 0x00, 0xFF, 0xFF,	// 10-13: data length
    0x00, 0x0A };		// response delay

//   'V'  cam#
//              31=write_date
//                    data_length
//                          SPI_eprom=4,
//                                # values/bytes to write?
//                                      address(word)
//                                                  value
//const uint8_t SET_SIZE640x480[] =
//  { 0x56, 0x00, 0x31, 0x05, 0x04, 0x01, 0x00, 0x19, 0x00 }; // SPI:0x0019 = 0x00
//const uint8_t SET_SIZE320x240[] =
//  { 0x56, 0x00, 0x31, 0x05, 0x04, 0x01, 0x00, 0x19, 0x11 }; // SPI:0x0019 = 0x11
//const uint8_t SET_SIZE160x120[] =
//  { 0x56, 0x00, 0x31, 0x05, 0x04, 0x01, 0x00, 0x19, 0x22 }; // SPI:0x0019 = 0x22
const uint8_t SET_COMPRESS[] =	// register 05?
  { 0x56, 0x00, 0x31, 0x05, 0x01, 0x01, 0x12, 0x04, 0xFF }; // ChipReg:0x1204 = 0x??
const uint8_t RESET_CAMERA[] =
  { 0x56, 0x00, 0x26, 0x00 };
const uint8_t PAUSE_FRAME[] =	// fbuf_ctrl command to stop current frame updating..
  { 0x56, 0x00, 0x36, 0x01, 0x00 };
//const uint8_t STEP_FRAME[] =
//  { 0x56, 0x00, 0x36, 0x01, 0x03 };
const uint8_t RESUME_FRAME[] =
  { 0x56, 0x00, 0x36, 0x01, 0x02 };
const uint8_t GET_FILESIZE[] =
  { 0x56, 0x00, 0x34, 0x01, 0x00 };
//const uint8_t WRITEDATAREG_OKAY[] =
//  { 0x76, 0x00, 0x31, 0x00, 0x00 };
//const uint8_t PACKET_OKAY[] =
//  { 0x76, 0x00, 0x32, 0x00, 0x00 };
const uint8_t READSIZE_OKAY[] =
  { 0x76, 0x00, 0x34, 0x00, 0x04 };
//const uint8_t COMMAND_OKAY[] =
//  { 0x76, 0x00, 0x36, 0x00, 0x00 };
//const uint8_t SET_COLOURAUTO[] =
//  { 0x56, 0x00, 0x3c, 0x02, 0x01, 0x00 };
//const uint8_t SET_COLOUR[] =
//  { 0x56, 0x00, 0x3c, 0x02, 0x01, 0x01 };
//const uint8_t SET_MONO[] =
//  { 0x56, 0x00, 0x3c, 0x02, 0x01, 0x02 };
//const uint8_t COLOURCTL_OKAY[] =
//  { 0x76, 0x00, 0x3c, 0x00, 0x00 };
//const uint8_t SET_INDOOR_ON[] =
//  { 0x56, 0x00, 0x40, 0x03, 0x01, 0x01, 0x01 };
//const uint8_t SET_INDOOR_AUTO[] =
//  { 0x56, 0x00, 0x40, 0x03, 0x01, 0x01, 0x00 };
//const uint8_t SET_BACKLIGHTCOMP_ON[] =
//  { 0x56, 0x00, 0x40, 0x03, 0x02, 0x01, 0x01 };
//const uint8_t SET_BACKLIGHTCOMP_OFF[] =
//  { 0x56, 0x00, 0x40, 0x03, 0x02, 0x01, 0x00 };
//const uint8_t AECTRL_OKAY[] =
//  { 0x76, 0x00, 0x40, 0x00, 0x00 };

// ----------------------------------------------------------------------

int write_and_listen (uint8_t *sendstr, int sendsize,
		      uint8_t *buffer, int rsize)
{
  if (sendstr != NULL) CAMERA.write (sendstr, sendsize);

  //digitalWrite (13, HIGH);
  //while (CAMERA.available() < rsize) continue;
  //digitalWrite (13, LOW);

  return CAMERA.readBytes (buffer, rsize);
}

// ----------------------------------------------------------------------

void camera_reset (void)
{
  uint8_t *p = buffer;

  CAMERA.write (RESET_CAMERA, 4);
  delay (20);			// pause for the actual reset

  while (1)			// suck down the initization banner
    if (CAMERA.available())
      if ((*p++ = (uint8_t)CAMERA.read()) == 10)
	if (!memcmp ("Init end\r\n", buffer, 10))
	  break;
	else
	  p = buffer;

  delay (10);
}

/* ------------------------------------------------------------------------- */

//void set_image_size (uint8_t *cmd)
//{
//  write_and_listen (cmd, 9, buffer, 5);
//  camera_reset();
//}

/* ------------------------------------------------------------------------- */

//int set_colour (uint8_t *cmd)
//{
//  return write_and_listen (cmd, 6, buffer, 5);
//}

/* ------------------------------------------------------------------------- */

//int set_ae (uint8_t *cmd)
//{
//  return write_and_listen (cmd, 7, buffer, 5);
//}

/* ------------------------------------------------------------------------- */

int set_compression (uint8_t factor)
{
  memcpy (buffer, SET_COMPRESS, 9);
  buffer[8] = factor;
  return write_and_listen (buffer, 9, buffer, 5);
}

/* ------------------------------------------------------------------------- */

int pause_frame (void)
{
  return write_and_listen (PAUSE_FRAME, 5, buffer, 5);
}

int resume_frame (void)
{
  return write_and_listen (RESUME_FRAME, 5, buffer, 5);
}

//int step_frame (void)
//{
//  return write_and_listen (STEP_FRAME, 5, buffer, 5);
//}

/* ------------------------------------------------------------------------- */

uint32_t read_jpeg_size (void)
{
  write_and_listen (GET_FILESIZE, 5, buffer, 9);
  if (memcmp (buffer, READSIZE_OKAY, 5) != 0)
    return 0;

  return ((uint32_t)buffer[7] << 8) + (uint32_t)buffer[8];
}

// ----------------------------------------------------------------------
// 0 160x120 greyscale high compression
// 1 160x120 greyscale low compression
// 2 160x120 colour high compression - 'green'
// 3 160x120 colour low compression
// 4 320x240 greyscale low compression
// 5 320x240 colour low compression
// 6 640x480 greyscale high compression
// 7 640x480 greyscale low compression
// 8 640x480 colour high compression
// 9 640x480 colour low compression

//const uint8_t* setsizes[] =
//  { SET_SIZE160x120, SET_SIZE160x120, SET_SIZE160x120, SET_SIZE160x120,
//    SET_SIZE320x240, SET_SIZE320x240, SET_SIZE640x480, SET_SIZE640x480,
//    SET_SIZE640x480, SET_SIZE640x480 };

//const uint8_t* setcolourmode[] =
//  { SET_MONO, SET_MONO, SET_COLOUR, SET_COLOUR,
//    SET_MONO, SET_COLOUR, SET_MONO, SET_MONO,
//    SET_COLOUR, SET_COLOUR };

const uint8_t setcompression[] =
  { 255, 232, 208, 184, 160, 136, 112, 88, 64, 40 };

// 40 = ~62kB
// 99 - ~28kB
// 128 - ~21kB
// 250 - ~12kB

void do_photo (int opt)
{
  unsigned long photostart = millis();

  camera_reset();
  //set_image_size (setsizes[opt]); // doesn't work?
  //set_colour (setcolourmode[opt]); // also nowerk
  set_compression (setcompression[opt]);
  //set_ae (SET_BACKLIGHTCOMP_ON);
  //set_ae (SET_50HZ);
  //set_ae (SET_INDOOR_ON);

  //

  delay (2000);			// for autoexposure, etc.
  pause_frame();

  digitalWrite (13, HIGH);
  radio.stopListening();

  uint32_t filesize = read_jpeg_size();
  if (!filesize)
    {
      memset (payload, 0xFF, PACKETSIZE);
      payload[2] = 1;
      radio.write (payload, PACKETSIZE);
      radio.startListening();
      return;
    }

  //

  int chunk = PACKETSIZE-2, r = 0;
  uint32_t pointer = 0;
  uint16_t checksum = 0;
  uint8_t reason = 0xFF;

  while (pointer < filesize)
    {
      memset (payload, 0xFF, PACKETSIZE);

      READ_FBUF[8] = pointer >> 8; READ_FBUF[9] = pointer & 255;
      READ_FBUF[12] = 0; READ_FBUF[13] = chunk;

      int r = write_and_listen (READ_FBUF, 16, buffer, 10+chunk);

      //if (buffer[4] == 1)
      //continue;
      //if (r != chunk+10)
      //	continue;

      payload[0] = pointer >> 8;
      payload[1] = pointer & 255;
      memcpy (payload+2, buffer+5, chunk);

      for (int i = 0; i < chunk; i++) checksum += buffer[5+i];

      int retries = 8;
      while (retries && !radio.write (payload, PACKETSIZE)) retries--;
      if (retries == 0) break;
      if ((millis() - photostart) > 60000) break;
      pointer += chunk;
    }

  // EOF packet

  memset (payload, reason, PACKETSIZE);
  payload[0] = 0xFF; payload[1] = 0xFF;
  payload[2] = r;
  payload[3] = chunk;
  payload[4] = filesize >> 8;
  payload[5] = filesize & 255;
  payload[6] = pointer >> 8;
  payload[7] = pointer & 255;
  payload[8] = checksum >> 8;
  payload[9] = checksum & 255;
  radio.write (payload, PACKETSIZE);
  radio.startListening();
  resume_frame();
  return;
}

// ----------------------------------------------------------------------

void process_cmd (char cmd, char time)
{
  static int motorspeed = 80;
  int tl = -1, tr = -1;

  switch (cmd)
    {
      // case 'F': forward until proximity = <time>; "0"=5cm; "9"=1m
    case 'p': do_photo (time); break;
    case 's': motorspeed = 60 + (20 * (int)time); break;
    case 'f': tl = FORWARD; tr = FORWARD; break;
    case 'b': tl = BACKWARD; tr = BACKWARD; break;
    case 'l': tl = FORWARD; tr = BRAKE; break;
    case 'L': tl = FORWARD; tr = BACKWARD; break;
    case 'r': tl = BRAKE; tr = FORWARD; break;
    case 'R': tl = BACKWARD; tr = FORWARD; break;
    default: tl = BRAKE; tr = BRAKE;
    }

  if (tl > -1)
    {
      motor.control (TRACKLEFT, tl, motorspeed);
      motor.control (TRACKRIGHT, tr, motorspeed);

      do
	// todo: monitor tilt sensor and abort if activated
	delay (time > 0 ? 1000 : 333);
      while (time--);

      motor.control (TRACKLEFT, RELEASE, motorspeed);
      motor.control (TRACKRIGHT, RELEASE, motorspeed);
    }

  // could also report proximity, tilt
  proximity();
}

// ----------------------------------------------------------------------

void setup (void)
{
  pinMode (DISTSEN, INPUT);

  CAMERA.begin (38400);
  while (!CAMERA) continue;
  CAMERA.setTimeout (250);

  while (!radio.begin()) delay (10);

  radio.setChannel (CHANNEL);
  radio.setPayloadSize (PACKETSIZE);
  //radio.setRetries (5, 15);
  radio.setDataRate (RF24_250KBPS);
  radio.setPALevel (RF24_PA_HIGH, 1);
  radio.setCRCLength (RF24_CRC_16);
  radio.setAutoAck (true);
  radio.setAddressWidth (4);
  radio.openWritingPipe (address);
  radio.openReadingPipe (1, address);
  radio.txDelay = 75;		// discovered value 75

  radio.stopListening();
  radio.write ("wan wan!                  ", PACKETSIZE);
  radio.startListening();
}

// ----------------------------------------------------------------------

void loop (void)
{
  static unsigned long lastpacket = millis();

  //  if ((millis() - lastpacket) > 30000)
  //  {
      // LOS situtation
  //  }

  // incoming control packet from Controller-Mono
  if (radio.available())
    {
      radio.read (payload, 32);
      process_cmd (payload[0], payload[1]);
      //    lastpacket = millis();
    }
}

// ----------------------------------------------------------------------
